// 正逆运动学api示例
// 要先运行roscore和robot_state_publisher节点（display.launch）
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "kdl_kinematics_example");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // 加载机器人模型
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

  // 创建运动学状态
  robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
  kinematic_state->setToDefaultValues();
  
  const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("manipulator");//机械臂名称

  // 正运动学示例
  std::vector<double> joint_values = {0.1, 0.2, 0.3, 0.4, 0.5};
  kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
  // const Eigen::Isometry3d& end_effector_state = kinematic_state->getGlobalLinkTransform("end");//const不能修改
  Eigen::Isometry3d end_effector_state = kinematic_state->getGlobalLinkTransform("end");//末端坐标

  // 输出end_effector_state的值
  auto translation = end_effector_state.translation();
  auto rotation = Eigen::Quaterniond(end_effector_state.linear()); // 将旋转矩阵转换为四元数
  
  // 自定义位姿
  end_effector_state.translation().x() = 0.2;
  end_effector_state.translation().y() = 0.3;
  end_effector_state.translation().z() = 0.4;
  // 创建一个四元数来表示旋转
  Eigen::Quaterniond rotation_quaternion(1, 0, 0, 0); // 注意：四元数的构造函数顺序是(w, x, y, z)
  // 使用四元数更新end_effector_state的旋转部分
  end_effector_state.linear() = rotation_quaternion.toRotationMatrix();

  ROS_INFO("End effector position: x=%f, y=%f, z=%f", translation.x(), translation.y(), translation.z());
  // ROS_INFO("End effector orientation: x=%f, y=%f, z=%f, w=%f", rotation.x(), rotation.y(), rotation.z(), rotation.w());
  ROS_INFO("End effector orientation: x=%f, y=%f, z=%f, w=%f", 
         rotation_quaternion.x(), rotation_quaternion.y(), rotation_quaternion.z(), rotation_quaternion.w());
  // 给定joint_values正运动学用上面一行自定义用下面打印rotation
  // 使用下面一行需要注释掉自定义位姿和rosinfo orientation行 
  // 逆运动学示例
  // bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);
  bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 0.1);

  if (found_ik)
  {
    kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
    for(std::size_t i = 0; i < joint_values.size(); ++i)
    {
      // ROS_INFO("Joint %d: %f", i, joint_values[i]);
      ROS_INFO("Joint %zu: %f", i, joint_values[i]);
    }
  }
  else
  {
    ROS_INFO("Did not find IK solution");
  }

  ros::shutdown();
  return 0;
}
